
#include "nav_pkg/Navigate.h"

Navigate::Navigate(const std::string &name, const BT::NodeConfiguration &config): BT::ActionNode<nav2_msgs::action::Navigate_to_Pose>(name, config) 
    {

    }

// tick 传递参数

static BT::PortsList Navigate::providedPorts() 
{
  return {BT::InputPort<float>("goal")};
}

BT::NodeStatus Navigate::tick() override {
  auto client = this->node_->create_client<nav2_msgs::action::NavigateToPose>(
      "navigate_to_pose");
  while (!client->wait_for_action_server(std::chrono::seconds(1))) {
    if (!rclcpp::ok()) {
      return BT::NodeStatus::FAILURE;
    }

    RCLCPP_INFO(rclcpp::get_logger("behavior_tree"),
                "Waiting for the navigation action server to start.");
  }

  float goal;
  getInput("goal", goal) std::cout < "navigate to the pose" << goal;
  // do what you want
  sleep(5);

  return BT::NodeStatus::SUCCESS;
}
